# !/usr/bin/env python
# -*- coding: utf-8 -*-

from collections import deque
from decimal import Decimal


def bfs(graph, start, goal):
    # Initialize a queue for BFS
    queue = deque([(start, [start], 0)])  # (node, path, path_cost)
    visited = set()  # To keep track of visited nodes

    while queue:
        current_node, path, path_cost = queue.popleft()

        if current_node == goal:
            return path, path_cost

        visited.add(current_node)

        # Explore neighbors
        for neighbor, distance in graph[current_node].items():
            if neighbor not in visited:
                queue.append((neighbor, path + [neighbor], Decimal(str(path_cost)) + Decimal(str(distance))))

    return None, None  # No path found


import heapq


def optimal_bfs(graph, start, goal):
    # 优先队列：每个元素是 (当前路径的距离, 当前节点)
    pq = [(0, start)]  # (当前总权重, 当前节点)
    # 存储每个节点到起点的最短距离
    distances = {start: 0}
    # 存储每个节点的前驱节点，用于恢复路径
    previous_nodes = {start: None}
    # 存储已访问的节点
    visited = set()

    while pq:
        current_distance, current_node = heapq.heappop(pq)

        if current_node in visited:
            continue

        visited.add(current_node)

        # 如果目标节点已找到
        if current_node == goal:
            path = []
            while previous_nodes[current_node] is not None:
                path.append(current_node)
                current_node = previous_nodes[current_node]
            path.append(start)
            path.reverse()
            return path, round(distances[goal], 2)

        # 遍历邻居节点
        for neighbor, weight in graph[current_node].items():
            if neighbor in visited:
                continue

            new_distance = current_distance + weight
            if neighbor not in distances or new_distance < distances[neighbor]:
                distances[neighbor] = new_distance
                previous_nodes[neighbor] = current_node
                heapq.heappush(pq, (new_distance, neighbor))

    return None, float('inf')  # 如果没有路径，返回 None


if __name__ == '__main__':
    graph = {
        'TSW': {'YL': 3.89},
        'YL': {'TSW': 3.89, 'TM': 7.57, 'LMC': 10.92, 'KT': 3.89},
        'LMC': {'YL': 10.92, 'LW': 3.89, 'SS': 3.42, 'SK': 3.96},
        'LW': {'LMC': 3.89, 'SS': 1.34},
        'SS': {'LW': 1.34},
        'TM': {'YL': 7.57, 'TLC': 5.46},
        'KT': {'YL': 3.89, 'SK': 3.43, 'TW': 8.17},
        'SK': {'KT': 3.43, 'LMC': 8.96, 'TW': 7.41},
        'TLC': {'TM': 5.46, 'TW': 7.1},
        'TW': {'KT': 8.17, 'TLC': 7.1, 'SK': 7.41, 'KC': 3.9},
        'KC': {'TW': 3.9, 'TY': 3.43},
        'TY': {'KC': 3.43}
    }

    # BFS
    # bfs_path, bfs_cost = bfs(graph, 'LW', 'KT')
    # print(f"BFS Path: {bfs_path}, Total Distance: {bfs_cost} km")
    bfs_path, bfs_cost = optimal_bfs(graph, 'TSW', 'TY')
    print(f"BFS optima Path: {bfs_path}, Total Distance: {bfs_cost} km")

    h_dict = {}
    for k, v in graph.items():
        bfs_path, bfs_cost = optimal_bfs(graph, k, 'TY')
        h_dict[k] = bfs_cost
        print(f"BFS optima Path: {bfs_path}, Total Distance: {bfs_cost} km, from {k} to TY")
    print(h_dict)
